#ifndef SAMPLER_2D_H
#define SAMPLER_2D_H

#include "basic_typedef.h"
#include "CollisionDetector2D.h"

class Sampler2D {
public:
  ////////////////////////
  // CTORS
  ////////////////////////
  /*  Parameters:
      radius  - radius of robot
      Room  - describes the boundary of the scenario
      CollisionDetector  - Collision detection black box
  */

  Sampler2D(const Room& room, CollisionDetector2D *col, double seed = -1) : m_col(col) {  
    Point_2 tl = room.first;
    Point_2 br = room.second;
    xmin = CGAL::to_double(room.first.x());
    xmax = CGAL::to_double(room.second.x());
    ymax = CGAL::to_double(room.second.y());
    ymin = CGAL::to_double(room.first.y());
    lenx = xmax - xmin;
    leny = ymax - ymin;

    if (seed == -1) seed = time(NULL);
    srand(seed);
  }

  ~Sampler2D(void) { }

  ////////////////////////
  // Queries
  ////////////////////////
  
  /* Returns a point that represents a valid (free) configuration of the robot.
   */
  Point_2 generate_sample() const
  {
    {
      while(true) {
        // sample a point in the configuration space
        Point_2 p = generate_sample_no_obstacles();

        // validate sample using a collision detector
        if (m_col->valid_conf(p)) return p;
      }
    }
  }

private:
  /* Samples a point in the configuration space of the robot defined by the
   * bounding box of the scenario. This point can be valid or invalid.
   */
  Point_2 generate_sample_no_obstacles() const
	{
		double randx = xmin + lenx * ((double)rand())/((double)RAND_MAX);
		double randy = ymin + leny * ((double)rand())/((double)RAND_MAX);

		Point_2 p(randx,randy);

		return p;
	}

  ////////////////////////
  // Data Members
  ////////////////////////
    
  CollisionDetector2D* m_col;     // Collision detector

  // room variables
  double xmin;
  double xmax;
  double ymin;
  double ymax;
  double lenx;
  double leny;
};
#endif
